function LidarSLAM()
    %% LidarSLAM - 接收激光雷达数据并执行SLAM
    % 支持从串口或UDP接收数据
    % 使用方法：直接运行此函数，按照提示选择数据源
    
    %% 初始化
    clear;
    clc;
    
    %% 数据源设置
    availablePorts = serialportlist("available");
    if isempty(availablePorts)
        error('没有可用的串口设备');
    end
    
    % 显示可用串口
    fprintf('可用串口:\n');
    for i = 1:length(availablePorts)
        fprintf('%d: %s\n', i, availablePorts{i});
    end
    
    % 选择串口
    portIdx = input('请选择串口编号: ');
    baudRate = input('请输入波特率 (默认115200): ');
    if isempty(baudRate)
        baudRate = 115200;
    end
    
    % 打开串口
    serialObj = serialport(availablePorts{portIdx}, baudRate);
    
    fprintf('串口 %s 已打开，波特率 %d\n', availablePorts{portIdx}, baudRate);
    
    %% 初始化SLAM
    maxRange = input('请输入激光雷达最大范围(米) (默认12): ');
    if isempty(maxRange)
        maxRange = 12;
    end
    
    mapResolution = input('请输入地图分辨率(cells/meter) (默认20): ');
    if isempty(mapResolution)
        mapResolution = 20;
    end
    
    % 创建SLAM对象
    slamObj = lidarSLAM(mapResolution, maxRange);
    % 配置SLAM参数 - 优化性能
    slamObj.LoopClosureThreshold = 200;  % 降低回环检测阈值
    slamObj.LoopClosureSearchRadius = 8;
    
    % 内存管理变量
    maxScansToKeep = 30;  % 我们自己管理扫描数量
    scanCounter = 0;
    
    % 创建地图对象
    mapSize = 20;  % 初始地图大小 (米)
    occupancyMapObj = occupancyMap(mapSize, mapSize, mapResolution);
    
    %% 初始化可视化
    fig = figure('Name', '激光雷达SLAM', 'NumberTitle', 'off', 'Position', [100, 100, 1200, 800]);
    
    % 设置按键回调函数
    set(fig, 'KeyPressFcn', @keyPressCallback);
    
    % 地图显示
    subplot(2, 2, [1,3]);
    mapAxes = gca;
    hMap = imagesc(zeros(100, 100));
    colormap gray;
    hold on;
    hRobot = plot(0, 0, 'ro', 'MarkerSize', 10, 'LineWidth', 2);
    hPath = plot(0, 0, 'r-', 'LineWidth', 1);
    hScan = plot(0, 0, 'g.', 'MarkerSize', 2);
    title('SLAM地图和机器人位置');
    axis equal;
    
    % 原始点云显示
    subplot(2, 2, 2);
    hPolar = polarplot(0, 0, 'g.', 'MarkerSize', 4);
    title('原始激光雷达数据');
    
    % 信息显示
    subplot(2, 2, 4);
    hText = text(0.1, 0.9, '等待数据...', 'FontSize', 12);
    axis off;
    
    % 添加数据收集按钮
    uicontrol('Style', 'pushbutton', 'String', '强制处理当前数据', ...
              'Position', [10, 10, 150, 30], ...
              'Callback', @processCurrentData);
    
    uicontrol('Style', 'pushbutton', 'String', '注入测试数据', ...
              'Position', [170, 10, 100, 30], ...
              'Callback', @injectTestData);
    
    %% 激光雷达数据缓存
    angles = [];        % 角度 (度)
    distances = [];     % 距离 (米)
    
    %% 全局变量
    running = true;
    scanCount = 0;
    lastUpdateTime = tic;
    hasValidPoses = false;  % 标记是否有有效的位姿数据
    
    % 路径跟踪
    robotPath = [];     % 存储机器人路径
    maxPathPoints = 500; % 限制路径点数量
    
    % 性能参数
    pointDecimationFactor = 2;  % 激光点抽取因子
    scanProcessInterval = 5;    % 扫描处理间隔
    mapUpdateInterval = 10;     % 地图更新间隔
    visualUpdateRate = 5;       % 可视化更新率 (Hz)
    
    % 调试选项
    enableDebugOutput = false;  % 默认关闭调试信息
    minPointsForScan = 10;     % 处理扫描所需的最少点数
    collectDataMode = false;   % 数据收集模式
    collectedData = [];        % 收集的原始数据
    
    % 解析模式选择
    parseMode = menu('选择数据解析模式', 'RPLIDAR标准格式', '自定义解析逻辑(3字节)', '自定义解析逻辑(2字节)');
    
    %% 性能配置对话框
    perfFig = figure('Name', '性能配置', 'Position', [300, 300, 400, 300], 'MenuBar', 'none', 'ToolBar', 'none');
    
    % 激光点抽取因子
    uicontrol(perfFig, 'Style', 'text', 'String', '激光点抽取因子:', 'Position', [20, 250, 150, 20]);
    hDecimation = uicontrol(perfFig, 'Style', 'slider', 'Min', 1, 'Max', 10, 'Value', pointDecimationFactor, ...
                        'Position', [180, 250, 150, 20], 'Callback', @updateDecimation);
    hDecimationText = uicontrol(perfFig, 'Style', 'text', 'String', num2str(pointDecimationFactor), 'Position', [340, 250, 30, 20]);
    
    % 扫描处理间隔
    uicontrol(perfFig, 'Style', 'text', 'String', '扫描处理间隔:', 'Position', [20, 200, 150, 20]);
    hScanInterval = uicontrol(perfFig, 'Style', 'slider', 'Min', 1, 'Max', 20, 'Value', scanProcessInterval, ...
                         'Position', [180, 200, 150, 20], 'Callback', @updateScanInterval);
    hScanIntervalText = uicontrol(perfFig, 'Style', 'text', 'String', num2str(scanProcessInterval), 'Position', [340, 200, 30, 20]);
    
    % 地图更新间隔
    uicontrol(perfFig, 'Style', 'text', 'String', '地图更新间隔:', 'Position', [20, 150, 150, 20]);
    hMapInterval = uicontrol(perfFig, 'Style', 'slider', 'Min', 1, 'Max', 30, 'Value', mapUpdateInterval, ...
                       'Position', [180, 150, 150, 20], 'Callback', @updateMapInterval);
    hMapIntervalText = uicontrol(perfFig, 'Style', 'text', 'String', num2str(mapUpdateInterval), 'Position', [340, 150, 30, 20]);
    
    % 可视化更新率
    uicontrol(perfFig, 'Style', 'text', 'String', '可视化更新率 (Hz):', 'Position', [20, 100, 150, 20]);
    hVisualRate = uicontrol(perfFig, 'Style', 'slider', 'Min', 1, 'Max', 30, 'Value', visualUpdateRate, ...
                      'Position', [180, 100, 150, 20], 'Callback', @updateVisualRate);
    hVisualRateText = uicontrol(perfFig, 'Style', 'text', 'String', num2str(visualUpdateRate), 'Position', [340, 100, 30, 20]);
    
    % 完成按钮
    uicontrol(perfFig, 'Style', 'pushbutton', 'String', '确定', 'Position', [150, 20, 100, 30], ...
             'Callback', @closePerfConfig);
    
    % 等待用户配置完成
    uiwait(perfFig);
    
    %% 主循环
    disp('开始接收激光雷达数据...');
    disp('按 q 退出，按 s 保存地图，按 r 重置SLAM，按 d 切换调试信息，按 c 切换数据收集模式，按 p 打开性能配置');
    
    buffer = uint8([]);
    lastDataTime = tic;
    lastProcessTime = tic;
    lastMapUpdateTime = tic;
    performanceStats = struct('processTime', [], 'updateTime', [], 'scanPoints', []);
    
    while running && ishandle(fig)  % 检查图形窗口是否仍然存在
        % 接收串口数据
        if serialObj.NumBytesAvailable > 0
            % 读取所有可用字节
            data = read(serialObj, serialObj.NumBytesAvailable, "uint8");
            
            % 追加到缓冲区
            buffer = [buffer; data];
            
            % 数据收集模式
            if collectDataMode
                collectedData = [collectedData; data];
                if length(collectedData) > 1000  % 限制收集的数据量
                    collectDataMode = false;
                    saveCollectedData();
                end
            end
            
            % 记录最后一次数据接收时间
            lastDataTime = tic;
            
            if enableDebugOutput
                fprintf('接收到 %d 字节数据\n', length(data));
            end
            
            % 处理数据
            switch parseMode
                case 1  % RPLIDAR标准格式
                    parseRPLIDARData(buffer);
                case 2  % 自定义解析逻辑(3字节)
                    parseCustomData3Bytes(buffer);
                case 3  % 自定义解析逻辑(2字节)
                    parseCustomData2Bytes(buffer);
            end
        end
        
        % 定期更新显示 (根据配置的更新率)
        if toc(lastUpdateTime) > 1/visualUpdateRate
            updateVisualization();
            lastUpdateTime = tic;
        end
        
        % 如果超过1秒没有接收到数据，清空缓冲区
        if toc(lastDataTime) > 1
            buffer = uint8([]);
        end
        
        % 短暂暂停，避免CPU占用过高
        pause(0.001);
    end
    
    %% 清理
    clear serialObj;
    disp('程序已退出');
    
    %% 性能配置回调函数
    function updateDecimation(src, ~)
        pointDecimationFactor = round(get(src, 'Value'));
        set(hDecimationText, 'String', num2str(pointDecimationFactor));
    end
    
    function updateScanInterval(src, ~)
        scanProcessInterval = round(get(src, 'Value'));
        set(hScanIntervalText, 'String', num2str(scanProcessInterval));
    end
    
    function updateMapInterval(src, ~)
        mapUpdateInterval = round(get(src, 'Value'));
        set(hMapIntervalText, 'String', num2str(mapUpdateInterval));
    end
    
    function updateVisualRate(src, ~)
        visualUpdateRate = round(get(src, 'Value'));
        set(hVisualRateText, 'String', num2str(visualUpdateRate));
    end
    
    function closePerfConfig(~, ~)
        delete(perfFig);
    end
    
    %% 按键回调函数
    function keyPressCallback(~, event)
        switch event.Key
            case 'q'  % 退出
                running = false;
                disp('正在退出程序...');
            case 's'  % 保存地图
                try
                    if hasValidPoses
                        [file, path] = uiputfile('*.mat', '保存地图');
                        if file ~= 0
                            map = occupancyMapObj;
                            save(fullfile(path, file), 'map');
                            disp(['地图已保存到: ', fullfile(path, file)]);
                        end
                    else
                        disp('尚无有效地图可保存');
                    end
                catch e
                    disp(['保存地图时出错: ', e.message]);
                end
            case 'r'  % 重置SLAM
                slamObj = lidarSLAM(mapResolution, maxRange);
                slamObj.LoopClosureThreshold = 200;
                slamObj.LoopClosureSearchRadius = 8;
                scanCounter = 0;  % 重置内存管理计数器
                occupancyMapObj = occupancyMap(mapSize, mapSize, mapResolution);
                hasValidPoses = false;
                scanCount = 0;
                robotPath = [];
                performanceStats = struct('processTime', [], 'updateTime', [], 'scanPoints', []);
                disp('SLAM已重置');
            case 'd'  % 切换调试信息
                enableDebugOutput = ~enableDebugOutput;
                if enableDebugOutput
                    disp('调试信息已启用');
                else
                    disp('调试信息已禁用');
                end
            case 'c'  % 切换数据收集模式
                collectDataMode = ~collectDataMode;
                if collectDataMode
                    collectedData = [];
                    disp('数据收集模式已启用，收集中...');
                else
                    disp('数据收集模式已禁用');
                    saveCollectedData();
                end
            case 'p'  % 打开性能配置对话框
                perfFig = figure('Name', '性能配置', 'Position', [300, 300, 400, 300], 'MenuBar', 'none', 'ToolBar', 'none');
                
                % 激光点抽取因子
                uicontrol(perfFig, 'Style', 'text', 'String', '激光点抽取因子:', 'Position', [20, 250, 150, 20]);
                hDecimation = uicontrol(perfFig, 'Style', 'slider', 'Min', 1, 'Max', 10, 'Value', pointDecimationFactor, ...
                                    'Position', [180, 250, 150, 20], 'Callback', @updateDecimation);
                hDecimationText = uicontrol(perfFig, 'Style', 'text', 'String', num2str(pointDecimationFactor), 'Position', [340, 250, 30, 20]);
                
                % 扫描处理间隔
                uicontrol(perfFig, 'Style', 'text', 'String', '扫描处理间隔:', 'Position', [20, 200, 150, 20]);
                hScanInterval = uicontrol(perfFig, 'Style', 'slider', 'Min', 1, 'Max', 20, 'Value', scanProcessInterval, ...
                                     'Position', [180, 200, 150, 20], 'Callback', @updateScanInterval);
                hScanIntervalText = uicontrol(perfFig, 'Style', 'text', 'String', num2str(scanProcessInterval), 'Position', [340, 200, 30, 20]);
                
                % 地图更新间隔
                uicontrol(perfFig, 'Style', 'text', 'String', '地图更新间隔:', 'Position', [20, 150, 150, 20]);
                hMapInterval = uicontrol(perfFig, 'Style', 'slider', 'Min', 1, 'Max', 30, 'Value', mapUpdateInterval, ...
                                   'Position', [180, 150, 150, 20], 'Callback', @updateMapInterval);
                hMapIntervalText = uicontrol(perfFig, 'Style', 'text', 'String', num2str(mapUpdateInterval), 'Position', [340, 150, 30, 20]);
                
                % 可视化更新率
                uicontrol(perfFig, 'Style', 'text', 'String', '可视化更新率 (Hz):', 'Position', [20, 100, 150, 20]);
                hVisualRate = uicontrol(perfFig, 'Style', 'slider', 'Min', 1, 'Max', 30, 'Value', visualUpdateRate, ...
                                  'Position', [180, 100, 150, 20], 'Callback', @updateVisualRate);
                hVisualRateText = uicontrol(perfFig, 'Style', 'text', 'String', num2str(visualUpdateRate), 'Position', [340, 100, 30, 20]);
                
                % 性能数据显示
                if ~isempty(performanceStats.processTime)
                    avgProcessTime = mean(performanceStats.processTime);
                    avgUpdateTime = mean(performanceStats.updateTime);
                    avgPoints = mean(performanceStats.scanPoints);
                    
                    uicontrol(perfFig, 'Style', 'text', 'String', sprintf('平均处理时间: %.2f ms', avgProcessTime*1000), ...
                             'Position', [20, 70, 350, 20], 'HorizontalAlignment', 'left');
                    uicontrol(perfFig, 'Style', 'text', 'String', sprintf('平均更新时间: %.2f ms', avgUpdateTime*1000), ...
                             'Position', [20, 50, 350, 20], 'HorizontalAlignment', 'left');
                    uicontrol(perfFig, 'Style', 'text', 'String', sprintf('平均点数: %.0f', avgPoints), ...
                             'Position', [20, 30, 350, 20], 'HorizontalAlignment', 'left');
                end
                
                % 完成按钮
                uicontrol(perfFig, 'Style', 'pushbutton', 'String', '确定', 'Position', [150, 20, 100, 30], ...
                         'Callback', @closePerfConfig);
        end
    end
    
    %% 强制处理当前数据
    function processCurrentData(~, ~)
        if ~isempty(angles) && ~isempty(distances)
            disp(['强制处理当前 ', num2str(length(angles)), ' 个数据点']);
            processScan(true);
        else
            disp('当前没有可用数据点');
        end
    end
    
    %% 保存收集的数据
    function saveCollectedData()
        if ~isempty(collectedData)
            try
                [file, path] = uiputfile('*.mat', '保存收集的数据');
                if file ~= 0
                    rawData = collectedData;
                    save(fullfile(path, file), 'rawData');
                    disp(['已保存 ', num2str(length(collectedData)), ' 字节数据到: ', fullfile(path, file)]);
                end
            catch e
                disp(['保存数据时出错: ', e.message]);
            end
        end
    end
    
    %% 注入测试数据 (仅用于测试)
    function injectTestData(~, ~)
        % 生成360个点的测试数据 (一个完整圆)
        testAngles = linspace(0, 359, 360);
        testDistances = 2 + 0.5*sin(deg2rad(testAngles*2)) + 0.2*randn(size(testAngles));
        
        % 保存到全局角度和距离数组
        angles = testAngles;
        distances = testDistances;
        
        % 处理一次扫描
        processScan(true);
        
        disp('已注入测试数据');
    end
    
    %% 解析RPLIDAR标准格式数据
    function parseRPLIDARData(data)
        % 标准RPLIDAR协议的解析
        state = 0;  % 0: 等待同步字节1, 1: 等待同步字节2, 2: 读取数据
        
        i = 1;
        while i <= length(data)
            switch state
                case 0  % 等待同步字节1 (0xA5)
                    if data(i) == hex2dec('A5')
                        state = 1;
                    end
                    i = i + 1;
                    
                case 1  % 等待同步字节2 (0x5A)
                    if i <= length(data)
                        if data(i) == hex2dec('5A')
                            state = 2;
                        else
                            state = 0;
                        end
                        i = i + 1;
                    else
                        buffer = data(i-1:end);  % 保留最后一个字节，以防是A5
                        return;
                    end
                    
                case 2  % 读取数据
                    % 标准RPLIDAR数据包为5字节
                    if i + 4 <= length(data)
                        % 解析数据
                        quality = bitshift(data(i), -2);  % 前6位是质量
                        
                        % 计算角度
                        angleQ6 = bitshift(bitand(data(i), 3), 6) + data(i+1);
                        angle = double(angleQ6) / 64.0;
                        
                        % 计算距离 (毫米)
                        distance = double(bitshift(uint16(data(i+3)), 8) + uint16(data(i+2))) / 4.0;
                        
                        if distance > 0 && quality > 0
                            angles(end+1) = angle;
                            distances(end+1) = distance / 1000.0;  % 转换为米
                        end
                        
                        i = i + 5;
                        state = 0;
                    else
                        buffer = data(i-1:end);  % 保留未完成的数据包
                        return;
                    end
            end
        end
        
        buffer = uint8([]);  % 所有数据已处理完毕
        
        % 检查是否有足够的数据点进行扫描，并且是否到了处理间隔
        if length(angles) >= minPointsForScan && toc(lastProcessTime) >= scanProcessInterval/10
            processScan(false);
            lastProcessTime = tic;
        end
    end
    
    %% 解析自定义数据格式 (3字节一组)
    function parseCustomData3Bytes(data)
        % 清空缓冲区以避免累积
        buffer = uint8([]);
        
        % 尝试不同的解析方式，这里假设每组是3字节，第1字节是角度索引，第2-3字节是距离
        if enableDebugOutput
            disp(['缓冲区长度: ', num2str(length(data))]);
        end
        
        % 清空角度和距离数组
        angles = [];
        distances = [];
        
        % 尝试找到有效的数据模式，每pointDecimationFactor个点取1个
        for i = 1:3*pointDecimationFactor:length(data)-2
            byte1 = double(data(i));
            byte2 = double(data(i+1));
            byte3 = double(data(i+2));
            
            % 假设第一个字节是角度索引 (0-255)，映射到0-360度
            angle = byte1 * 360 / 256;
            
            % 假设后两个字节是距离值 (高位在前)
            distance = (byte2 * 256 + byte3) / 1000.0;  % 转换为米
            
            % 过滤无效数据
            if distance > 0.1 && distance < maxRange
                angles(end+1) = angle;
                distances(end+1) = distance;
            end
        end
        
        if enableDebugOutput && ~isempty(angles)
            disp(['解析出 ', num2str(length(angles)), ' 个有效数据点']);
            disp(['角度范围: ', num2str(min(angles)), ' - ', num2str(max(angles))]);
            disp(['距离范围: ', num2str(min(distances)), ' - ', num2str(max(distances))]);
        end
        
        % 检查是否有足够的数据点进行扫描，并且是否到了处理间隔
        if length(angles) >= minPointsForScan && toc(lastProcessTime) >= scanProcessInterval/10
            processScan(false);
            lastProcessTime = tic;
        end
    end
    
    %% 解析自定义数据格式 (2字节一组)
    function parseCustomData2Bytes(data)
        % 清空缓冲区以避免累积
        buffer = uint8([]);
        
        % 清空角度和距离数组
        angles = [];
        distances = [];
        
        % 尝试找到有效的数据模式，每pointDecimationFactor个点取1个
        for i = 1:2*pointDecimationFactor:length(data)-1
            byte1 = double(data(i));
            byte2 = double(data(i+1));
            
            % 假设第一个字节是角度索引 (0-255)，映射到0-360度
            angle = byte1 * 360 / 256;
            
            % 假设第二个字节是距离值，单位为厘米
            distance = byte2 / 100.0;  % 转换为米
            
            % 过滤无效数据
            if distance > 0.1 && distance < maxRange
                angles(end+1) = angle;
                distances(end+1) = distance;
            end
        end
        
        if enableDebugOutput && ~isempty(angles)
            disp(['解析出 ', num2str(length(angles)), ' 个有效数据点']);
            if ~isempty(angles)
                disp(['角度范围: ', num2str(min(angles)), ' - ', num2str(max(angles))]);
                disp(['距离范围: ', num2str(min(distances)), ' - ', num2str(max(distances))]);
            end
        end
        
        % 检查是否有足够的数据点进行扫描，并且是否到了处理间隔
        if length(angles) >= minPointsForScan && toc(lastProcessTime) >= scanProcessInterval/10
            processScan(false);
            lastProcessTime = tic;
        end
    end
    
    %% 嵌套函数：处理一次完整的扫描
    function processScan(forceProcess)
        try
            % 记录开始时间
            startTime = tic;
            
            % 如果数据点太多，进行均匀抽样以提高性能
            if length(angles) > 360
                % 均匀抽样到大约360个点
                sampleStep = max(1, floor(length(angles) / 360));
                sampleIdx = 1:sampleStep:length(angles);
                sampleAngles = angles(sampleIdx);
                sampleDistances = distances(sampleIdx);
            else
                sampleAngles = angles;
                sampleDistances = distances;
            end
            
            % 创建激光扫描对象
            scan = lidarScan(sampleDistances, deg2rad(sampleAngles));
            
            % 添加到SLAM
            addScan(slamObj, scan);
            scanCount = scanCount + 1;
            scanCounter = scanCounter + 1;
            
            % 内存管理：如果扫描数量超过限制，重置SLAM对象但保留地图
            if scanCounter >= maxScansToKeep
                if enableDebugOutput
                    disp('重置SLAM对象以管理内存...');
                end
                
                % 获取当前位姿
                [~, poses] = scansAndPoses(slamObj);
                currentPose = poses(end,:);
                
                % 创建新的SLAM对象
                slamObj = lidarSLAM(mapResolution, maxRange);
                slamObj.LoopClosureThreshold = 200;
                slamObj.LoopClosureSearchRadius = 8;
                
                % 重置计数器
                scanCounter = 0;
                
                % 添加一个初始扫描，位置在当前位置
                addScan(slamObj, scan, currentPose);
            end
            
            % 记录处理时间
            processTime = toc(startTime);
            performanceStats.processTime(end+1) = processTime;
            performanceStats.scanPoints(end+1) = length(sampleAngles);
            
            % 限制性能数据数组大小
            if length(performanceStats.processTime) > 100
                performanceStats.processTime = performanceStats.processTime(end-99:end);
                performanceStats.scanPoints = performanceStats.scanPoints(end-99:end);
            end
            
            if enableDebugOutput
                fprintf('扫描处理时间: %.2f ms\n', processTime * 1000);
            end
            
            % 检查是否有有效位姿
            try
                [~, poses] = scansAndPoses(slamObj);
                if ~isempty(poses)
                    hasValidPoses = true;
                    currentPose = poses(end,:);
                    
                    % 添加到路径历史
                    robotPath = [robotPath; currentPose];
                    
                    % 限制路径点数量
                    if size(robotPath, 1) > maxPathPoints
                        robotPath = robotPath(end-maxPathPoints+1:end, :);
                    end
                    
                    % 只在强制处理或达到地图更新间隔时更新地图
                    if forceProcess || toc(lastMapUpdateTime) >= mapUpdateInterval/10
                        updateMapStart = tic;
                        
                        % 将扫描点转换到世界坐标系
                        xs = sampleDistances .* cos(deg2rad(sampleAngles));
                        ys = sampleDistances .* sin(deg2rad(sampleAngles));
                        
                        % 转换点到世界坐标系
                        cosTheta = cos(currentPose(3));
                        sinTheta = sin(currentPose(3));
                        
                        % 预分配内存以提高性能
                        worldPoints = zeros(length(xs), 2);
                        
                        % 批量计算旋转和平移
                        rotatedX = xs * cosTheta - ys * sinTheta;
                        rotatedY = xs * sinTheta + ys * cosTheta;
                        worldPoints(:,1) = rotatedX + currentPose(1);
                        worldPoints(:,2) = rotatedY + currentPose(2);
                        
                        % 过滤掉无效点
                        validIdx = ~isnan(worldPoints(:,1)) & ~isnan(worldPoints(:,2));
                        worldPoints = worldPoints(validIdx,:);
                        
                        if ~isempty(worldPoints)
                            % 使用批处理方式更新地图
                            
                            % 标记占用点
                            setOccupancy(occupancyMapObj, worldPoints, ones(size(worldPoints,1),1));
                            
                            % 标记自由空间 - 从机器人位置到每个扫描点
                            % 为了效率，只对部分点进行处理
                            sampleIdx = 1:10:size(worldPoints,1);  % 每10个点取1个
                            
                            % 预分配内存以提高性能
                            robotPos = [currentPose(1), currentPose(2)];
                            allFreePoints = [];
                            
                            % 批量生成自由空间点
                            for i = sampleIdx
                                % 生成从机器人到扫描点的直线
                                dist = norm(worldPoints(i,:) - robotPos);
                                numPoints = max(3, ceil(dist * 2)); % 减少点数
                                
                                % 生成直线上的点
                                t = linspace(0, 0.95, numPoints)'; % 不包括终点
                                linePoints = robotPos * (1-t) + worldPoints(i,:) .* t;
                                
                                % 累积所有自由空间点
                                allFreePoints = [allFreePoints; linePoints];
                            end
                            
                            % 批量设置自由空间
                            if ~isempty(allFreePoints)
                                % 限制点数以提高性能
                                if size(allFreePoints, 1) > 1000
                                    idx = randperm(size(allFreePoints, 1), 1000);
                                    allFreePoints = allFreePoints(idx,:);
                                end
                                setOccupancy(occupancyMapObj, allFreePoints, zeros(size(allFreePoints,1),1));
                            end
                        end
                        
                        % 记录地图更新时间
                        updateTime = toc(updateMapStart);
                        performanceStats.updateTime(end+1) = updateTime;
                        
                        % 限制性能数据数组大小
                        if length(performanceStats.updateTime) > 100
                            performanceStats.updateTime = performanceStats.updateTime(end-99:end);
                        end
                        
                        if enableDebugOutput
                            fprintf('地图更新时间: %.2f ms\n', updateTime * 1000);
                        end
                        
                        lastMapUpdateTime = tic;
                    end
                    
                    if enableDebugOutput
                        disp(['获取到有效位姿，当前位置: (', num2str(currentPose(1)), ', ', ...
                              num2str(currentPose(2)), ', ', num2str(rad2deg(currentPose(3))), '°)']);
                    end
                end
            catch e
                if enableDebugOutput
                    disp(['获取位姿时出错: ', e.message]);
                end
            end
            
            % 清空点集，准备下一次扫描
            angles = [];
            distances = [];
            
            if enableDebugOutput
                disp(['成功处理扫描 #', num2str(scanCount)]);
            end
        catch e
            fprintf('处理扫描时出错: %s\n', e.message);
        end
    end
    
    %% 嵌套函数：更新可视化
    function updateVisualization()
        try
            % 更新极坐标图 (始终显示原始数据)
            if ~isempty(angles) && ~isempty(distances)
                % 如果点太多，抽样显示以提高性能
                if length(angles) > 360
                    sampleStep = max(1, floor(length(angles) / 360));
                    sampleIdx = 1:sampleStep:length(angles);
                    set(hPolar, 'ThetaData', deg2rad(angles(sampleIdx)), ...
                                'RData', distances(sampleIdx));
                else
                    set(hPolar, 'ThetaData', deg2rad(angles), ...
                                'RData', distances);
                end
            end
            
            % 更新信息文本 (始终显示)
            infoText = sprintf('扫描次数: %d\n点数: %d', ...
                               scanCount, length(angles));
            
            % 只有在有有效位姿时才更新SLAM相关显示
            if hasValidPoses
                % 获取地图数据
                mapMatrix = occupancyMapObj.getOccupancy();
                [mapHeight, mapWidth] = size(mapMatrix);
                
                % 更新地图显示
                set(hMap, 'CData', mapMatrix);
                axis(mapAxes, [0.5, mapWidth+0.5, 0.5, mapHeight+0.5]);
                
                % 获取当前位姿
                [~, poses] = scansAndPoses(slamObj);
                currentPose = poses(end,:);
                
                % 将世界坐标转换为地图坐标
                [robotMapX, robotMapY] = worldToMap(occupancyMapObj, currentPose(1), currentPose(2));
                
                % 更新机器人位置
                set(hRobot, 'XData', robotMapX, 'YData', robotMapY);
                
                % 更新路径
                if ~isempty(robotPath)
                    % 预分配内存
                    pathMapX = zeros(size(robotPath,1), 1);
                    pathMapY = zeros(size(robotPath,1), 1);
                    
                    % 批量转换路径坐标
                    [pathMapX, pathMapY] = worldToMap(occupancyMapObj, robotPath(:,1), robotPath(:,2));
                    set(hPath, 'XData', pathMapX, 'YData', pathMapY);
                end
                
                % 如果有当前扫描点，更新点云显示
                if ~isempty(angles) && ~isempty(distances) && hasValidPoses
                    % 计算扫描点在世界坐标系中的位置
                    % 如果点太多，抽样显示以提高性能
                    if length(angles) > 360
                        sampleStep = max(1, floor(length(angles) / 360));
                        sampleIdx = 1:sampleStep:length(angles);
                        xs = distances(sampleIdx) .* cos(deg2rad(angles(sampleIdx)));
                        ys = distances(sampleIdx) .* sin(deg2rad(angles(sampleIdx)));
                    else
                        xs = distances .* cos(deg2rad(angles));
                        ys = distances .* sin(deg2rad(angles));
                    end
                    
                    % 转换到世界坐标系
                    cosTheta = cos(currentPose(3));
                    sinTheta = sin(currentPose(3));
                    
                    % 批量计算旋转和平移
                    rotatedX = xs * cosTheta - ys * sinTheta;
                    rotatedY = xs * sinTheta + ys * cosTheta;
                    worldX = rotatedX + currentPose(1);
                    worldY = rotatedY + currentPose(2);
                    
                    % 转换到地图坐标系
                    [scanMapX, scanMapY] = worldToMap(occupancyMapObj, worldX, worldY);
                    set(hScan, 'XData', scanMapX, 'YData', scanMapY);
                end
                
                % 计算性能统计
                if ~isempty(performanceStats.processTime)
                    avgProcessTime = mean(performanceStats.processTime);
                    fps = 1 / avgProcessTime;
                    
                    if ~isempty(performanceStats.updateTime)
                        avgUpdateTime = mean(performanceStats.updateTime);
                    else
                        avgUpdateTime = 0;
                    end
                else
                    avgProcessTime = 0;
                    avgUpdateTime = 0;
                    fps = 0;
                end
                
                % 更新信息显示
                infoText = sprintf(['扫描次数: %d\n' ...
                                   '地图大小: %dx%d\n' ...
                                   '点数: %d\n' ...
                                   '处理: %.1f ms (%.1f Hz)\n' ...
                                   '更新: %.1f ms\n' ...
                                   '位置: (%.2f, %.2f, %.1f°)'], ...
                                   scanCount, ...
                                   mapHeight, mapWidth, ...
                                   length(angles), ...
                                   avgProcessTime*1000, fps, ...
                                   avgUpdateTime*1000, ...
                                   currentPose(1), currentPose(2), rad2deg(currentPose(3)));
            end
            
            % 更新信息文本
            set(hText, 'String', infoText);
            
            drawnow limitrate;
        catch e
            % 如果出现错误，只记录错误，不中断程序
            if enableDebugOutput
                fprintf('可视化更新错误: %s\n', e.message);
            end
        end
    end
    
    %% 辅助函数：世界坐标转地图栅格坐标
    function [gridX, gridY] = worldToMap(map, worldX, worldY)
        % 获取地图分辨率和原点
        resolution = map.Resolution;
        [originX, originY] = map.GridLocationInWorld;
        
        % 转换坐标 - 支持向量输入
        gridX = (worldX - originX) / resolution + 1;
        gridY = (worldY - originY) / resolution + 1;
    end
end
